#pragma once

#include <ros/ros.h>

#include <Eigen/Core>
#include "stair_ctrl/interface.hpp"
#include "stair_ctrl/MotionPlan.h"

template <typename T>
class stepPlanInterface
{
public:
    T phase_scalar, d_phase;
    Eigen::Matrix<T, 4, 1> motion_plan_phase_vec, phase_offset;
    Eigen::Matrix<T, 12, 1> end_effector_ref, end_effector_ref_prev, end_effector_body, end_effector_offset;
    Eigen::Matrix<T, 3, 1> rot_des;
    bool blind = true;
    bool planning = false;
    
    T duration = 0.5;
    T step_x, step_y, step_dy;

    ros::Publisher plan_pub;
    ros::Subscriber plan_sub;

    stepPlanInterface(int rate = 100, T period = 0.5)
    {
        phase_scalar = 0.0;
        d_phase = 2.0 * M_PI / (period * rate);
        duration = period;

        step_x = 0.0;
        step_y = 0.0;
        step_dy = 0.0;

        motion_plan_phase_vec.setZero();
        phase_offset.setZero();
        rot_des.setZero();

        end_effector_ref.setZero();
        end_effector_ref_prev.setZero();
        end_effector_body.setZero();
        end_effector_offset.setZero();

        motion_plan_phase_vec << 0.0, M_PI, M_PI, 0.0;
        phase_offset << 0.0, M_PI, M_PI, 0.0;
        rot_des << 0.0, 0.0, 1.0;

        end_effector_offset << 0.1881, -0.12675, -0.3,
                                0.1881, 0.12675, -0.3,
                                -0.1881, -0.12675, -0.3,
                                -0.1881, 0.12675, -0.3;
        end_effector_ref << end_effector_offset;
        end_effector_ref_prev << end_effector_offset;
        end_effector_body << end_effector_offset;
    }

    void init(ros::NodeHandle *nh)
    {
        auto hints = ros::TransportHints().tcpNoDelay();

        std::string plan_topic;
        nh->param<std::string>("plan_topic", plan_topic, "/map_manager/step_plan");

        plan_sub = nh->subscribe(plan_topic, 100,  &stepPlanInterface::step_plan_callback, this, hints);
        plan_pub = nh->advertise<stair_ctrl::MotionPlan>("vision_request", 100);
    }

    void update(robotInterface<T> &robot_interface, cmdInterface<T> &cmd_interface)
    {
        // update phase
        phase_scalar += d_phase;
        motion_plan_phase_vec = phase_offset + Eigen::Matrix<T, 4, 1>::Constant(phase_scalar);
        for (int i = 0; i < 4; ++i)
        {
            if (motion_plan_phase_vec[i] > 2.0 * M_PI)
            {
                motion_plan_phase_vec[i] -= 2.0 * M_PI;
            }
        }
        if (phase_scalar > 2.0 * M_PI)
        {
            phase_scalar -= 2.0 * M_PI;
        }

        // update step length
        step_x = cmd_interface.cmd[0] * duration * 0.25;
        step_y = cmd_interface.cmd[1] * duration * 0.25;
        step_dy = cmd_interface.cmd[2] * duration * 0.25 * end_effector_offset[0];

        if (isUpdateStepPlan())
        {
            blind ? updateBlind(cmd_interface) : updateVision(robot_interface, cmd_interface);
            std::cout << "Current mode: " << (blind ? "[blind]" : "[vision]") << " mode !" << std::endl;
        }
    }

    bool isUpdateStepPlan()
    {
        bool is_transit_duration = false;
        if (!blind)
        {
            is_transit_duration = (phase_scalar < 0.1 * M_PI || phase_scalar > 1.9 * M_PI) || (phase_scalar > 0.9 * M_PI && phase_scalar < 1.1 * M_PI);
        }
        else
        {
            is_transit_duration = phase_scalar < 0.1 * M_PI || (phase_scalar > 0.99 * M_PI && phase_scalar < 1.1 * M_PI);
        }
        if (is_transit_duration)
        {
            if (!planning)
            {
                planning = true;
                return true;
            }
            else
            {
                return false;
            }
        }
        else
        {
            planning = false;
            return false;
        }
    }

    void updateBlind(cmdInterface<T> &cmd_interface)
    {
        end_effector_ref_prev = end_effector_ref;
        Eigen::Matrix<T, 3, 1> tmp;
        tmp.setZero();
        if (phase_scalar < 0.5 * M_PI || phase_scalar > 1.5 * M_PI)
        {
            // 0, 3 stance; 1, 2 swing;
            tmp << step_x, step_y + step_dy, 0.0;
            end_effector_ref.template segment<3>(0) << end_effector_offset.template segment<3>(0) - tmp;
            end_effector_ref.template segment<3>(3) << end_effector_offset.template segment<3>(3) + tmp;
            tmp << step_x, step_y - step_dy, 0.0;
            end_effector_ref.template segment<3>(6) << end_effector_offset.template segment<3>(6) + tmp;
            end_effector_ref.template segment<3>(9) << end_effector_offset.template segment<3>(9) - tmp;
        }
        else
        {
            // 1, 2 stance; 0, 3 swing;
            tmp << step_x, step_y + step_dy, 0.0;
            end_effector_ref.template segment<3>(0) << end_effector_offset.template segment<3>(0) + tmp;
            end_effector_ref.template segment<3>(3) << end_effector_offset.template segment<3>(3) - tmp;
            tmp << step_x, step_y - step_dy, 0.0;
            end_effector_ref.template segment<3>(6) << end_effector_offset.template segment<3>(6) - tmp;
            end_effector_ref.template segment<3>(9) << end_effector_offset.template segment<3>(9) + tmp;
        }
        rot_des << 0.0, 0.0, 1.0;
    }

    void updateVision(robotInterface<T> &robot_interface, cmdInterface<T> &cmd_interface)
    {
        sendRequest(robot_interface, cmd_interface);
    }

    void sendRequest(robotInterface<T> &robot_interface, cmdInterface<T> &cmd_interface)
    {
        auto request = stair_ctrl::MotionPlan();
        auto foot_pos = robot_interface.getFootPosBody().template cast<float>();
        auto vel = cmd_interface.cmd.template cast<float>();

        for (size_t i = 0; i < 12; ++i)
        {
            request.foot_pos.at(i) = foot_pos(i);
        }
        for (size_t i = 0; i < 3; ++i)
        {
            request.vec3.at(i) = vel(i);
        }
        request.phase = static_cast<float>(phase_scalar);
        plan_pub.publish(request);
    }

    void step_plan_callback(const stair_ctrl::MotionPlan::ConstPtr &msg)
    {
        end_effector_ref_prev = end_effector_ref;
        for (size_t i = 0; i < 3; ++i)
        {
            rot_des(i) = msg->vec3.at(i);
        }
        for (size_t i = 0; i < 12; ++i)
        {
            end_effector_ref(i) = msg->foot_pos.at(i);
        }
    }

    void switchMode()
    {
        blind = !blind;
        std::cout << "Switching to " << (blind ? "[blind]" : "[vision]") << " mode !" << std::endl;
    }
};